from machine import Pin, I2C
import math 
u=[]
NormAccz=0
RC_PIT=0.0
RC_ROL=0.0
RC_YAW=0.0
Butter_20HZ_Parameter_Acce = [
	[-0.00777631271910257, 0.06445464557871, 0.443321667140393],
	[0.443321667140393, 0.06445464557871, -0.00777631271910257]]
RC_THROTTLE = 0
YM_Dead=100
Input_Butter=[]
Output_Butter=[]
class PIDController:
    def __init__(self, Kp, Ki, Kd ):
        self.Kp = Kp#比例项参数 Kp： 控制系统的灵敏度，如果设置太高，系统可能会过于敏感，引起振荡。如果设置太低，系统响应可能较慢。
        self.Ki = Ki#积分项参数 Ki： 控制系统的稳定性，用于消除系统静差。过高的积分项可能导致系统过度调整，产生超调。
        self.Kd =Kd #微分项参数 Kd： 控制系统的过冲和振荡，可以减缓系统的响应。然而，过高的微分项可能引入噪声。
        self.outD=0
        self.outI=0
        self.outP=0
        self.iLimit=0
        self.deriv=0
        self.integ=0
        self.prevError=0
        self.error=0
        self.desired=0

    def pidUpdate(self, measured,expect):
        output=0
        self.deriv = expect
        self.error = self.desired-measured
        self.integ += self.error
        if self.integ>self.iLimit:
            self.integ=self.iLimit
        else:
            if self.integ<self.iLimit:
                self.integ=-self.iLimit
        self.deriv=self.error-self.prevError
        self.outP=self.Kp*self.error
        self.outI=self.Ki *self.integ
        self.outD=self.Kd*self.deriv
        output=self.outD+self.outI+self.outP
        self.prevError=self.error
        return output
def LPButterworth(curr_input,Buffer, Parameter):
    LPB_Cnt = 0
    #Buffer[1]为Input_Butter
    #Buffer[0]为Output_Butter
    #Parameter[1]为b
    Buffer[1][2] = curr_input
    if (LPB_Cnt >= 100):
        Buffer[0][2] =Parameter[1][0] * Buffer[1][2] + Parameter[1][1] * Buffer[1][1] + Parameter[1][2] * Buffer[1][0] - Parameter[0][1] * Buffer[0][1] - Parameter[0][2] * Buffer[0][0]
    else:
        Buffer[0][2] = Buffer[1][2]
        LPB_Cnt+=1
    Buffer[1][0] = Buffer[1][1]
    Buffer[1][1] = Buffer[1][2]

    Buffer[0][0] = Buffer[0][1]
    Buffer[0][1] = Buffer[0][2]

    return  Buffer[0][2]
pid=PIDController()
def init_control(x):
    yaw_expect=0 
    pitch_expect=0
    roll_expect=0
    pitch_expect = RC_PIT / 40
    roll_expect = RC_ROL / 40
    yaw_expect = RC_YAW / 40
    LPButterworth(t,u[1],Butter_20HZ_Parameter_Acce)
    LPButterworth(t,u[0],Butter_20HZ_Parameter_Acce)
    if RC_THROTTLE > YM_Dead:
        pid_out_pitch = pid.pidUpdate(,Output_Butter[2], pitch_expect)
        pid_out_roll = pid.pidUpdate(,Output_Butter[2], roll_expect)
        pid_in_pitch = pid.pidUpdate(, (Gyr_rad.Y * RadtoDeg), pid_out_pitch); // 内环是角速度
        pid_in_roll = pid.pidUpdate(, -(Gyr_rad.X * RadtoDeg), pid_out_roll);
        pid_in_yaw = pid.pidUpdate(, (Gyr_rad.Z * RadtoDeg), yaw_expect);
    else :
        if (RC_THROTTLE < YM_Dead): 
            pid_in_Pitchinteg = 0
            pid_in_Rollinteg = 0
            pid_in_Yawinteg = 0
    if (RC_THROTTLE > YM_Dead):
        if (MODE == 0):
            MOTO1_PWM = RC_THROTTLE + pid_in_pitch - pid_in_roll - pid_in_yaw;
            MOTO2_PWM = RC_THROTTLE - pid_in_pitch - pid_in_roll + pid_in_yaw;
            MOTO3_PWM = RC_THROTTLE - pid_in_pitch + pid_in_roll - pid_in_yaw;
            MOTO4_PWM = RC_THROTTLE + pid_in_pitch + pid_in_roll + pid_in_yaw;
        else:
            MOTO1_PWM = Z_THRUST * 1.2 + RC_THROTTLE + pid_in_pitch - pid_in_roll - pid_in_yaw;
            MOTO2_PWM = Z_THRUST * 1.2 + RC_THROTTLE - pid_in_pitch - pid_in_roll + pid_in_yaw;
            MOTO3_PWM = Z_THRUST * 1.2 + RC_THROTTLE - pid_in_pitch + pid_in_roll - pid_in_yaw;
            MOTO4_PWM = Z_THRUST * 1.2 + RC_THROTTLE + pid_in_pitch + pid_in_roll + pid_in_yaw;

		

	if (RC_THROTTLE > YM_Dead)
	{ // 如果油门大于死区值，PWM的最低值为死区值，最高值为100
		// 限幅
		if (MOTO1_PWM < 0)
			MOTO1_PWM = 0;
		else if (MOTO1_PWM > 900)
			MOTO1_PWM = 900;
		if (MOTO2_PWM < 0)
			MOTO2_PWM = 0;
		else if (MOTO2_PWM > 900)
			MOTO2_PWM = 900;
		if (MOTO3_PWM < 0)
			MOTO3_PWM = 0;
		else if (MOTO3_PWM > 900)
			MOTO3_PWM = 900;
		if (MOTO4_PWM < 0)
			MOTO4_PWM = 0;
		else if (MOTO4_PWM > 900)
			MOTO4_PWM = 900;
	}
	else // 否则让电机的PWM等于油门值，逐步起飞
	{
		MOTO1_PWM = 0, MOTO2_PWM = 0, MOTO3_PWM = 0, MOTO4_PWM = 0;
	}

	ledc_motor_run((int)MOTO1_PWM, (int)MOTO2_PWM, (int)MOTO3_PWM, (int)MOTO4_PWM);